

#include "pastycam.h"
#include <cmath>


PastyCam::PastyCam()
{
	posx=posy=posz=hang=vang = 0.0;
	lastDx=lastDy=lastDz = 0.0f;
}

void PastyCam::update(float vx, float vy, float vz, float deltaHang, float deltaVang)
{
	lastDx = vx;
	lastDy = vy;
	lastDz = vz;

	hang += deltaHang;
	vang += deltaVang;

	double ch = cos(hang);
	double cv = cos(vang);
	double sh = sin(hang);
	double sv = sin(vang);

	double rx[16];
	rx[0] = ch;  rx[2] = cv*sh;		
	rx[4] = 0;	 rx[6] = sv;		  
	rx[8] =-sh;  rx[10] = cv*ch;	
	// ydir=zdir cross xdir:
	rx[1] = rx[ 6]*rx[ 8] - rx[10]*rx[ 4]; // c.x=a.y*b.z-a.z*b.y
	rx[5] = rx[10]*rx[ 0] - rx[ 2]*rx[ 8]; // c.y=a.z*b.x-a.x*b.z
	rx[9] = rx[ 2]*rx[ 4] - rx[ 6]*rx[ 0]; // c.z=a.x*b.y-a.y*b.x

	rx[3] = 0.0; rx[7] = 0.0; rx[11] = 0.0;
	rx[12] = 0.0; rx[13] = 0.0; rx[14] = 0.0; rx[15] = 1.0;
	
	// Move cam in view direction
	posx += rx[0]*vx + rx[1]*vy + rx[2]*vz;
	posy += rx[4]*vx + rx[5]*vy + rx[6]*vz;
	posz += rx[8]*vx + rx[9]*vy + rx[10]*vz;

	// Translation matrix
	double tx[16] = {
	  1, 0, 0, 0,
	  0, 1, 0, 0,
	  0, 0, 1, 0,
	  -posx,  -posy,  -posz, 1
	};

	// Matrix mult, view = rx * translation
	modelview[ 0] = rx[0]*tx[ 0] + rx[4]*tx[ 1] + rx[ 8]*tx[ 2] + rx[12]*tx[ 3];
	modelview[ 1] = rx[1]*tx[ 0] + rx[5]*tx[ 1] + rx[ 9]*tx[ 2] + rx[13]*tx[ 3];
	modelview[ 2] = rx[2]*tx[ 0] + rx[6]*tx[ 1] + rx[10]*tx[ 2] + rx[14]*tx[ 3];
	modelview[ 3] = rx[3]*tx[ 0] + rx[7]*tx[ 1] + rx[11]*tx[ 2] + rx[15]*tx[ 3];

	modelview[ 4] = rx[0]*tx[ 4] + rx[4]*tx[ 5] + rx[ 8]*tx[ 6] + rx[12]*tx[ 7];
	modelview[ 5] = rx[1]*tx[ 4] + rx[5]*tx[ 5] + rx[ 9]*tx[ 6] + rx[13]*tx[ 7];
	modelview[ 6] = rx[2]*tx[ 4] + rx[6]*tx[ 5] + rx[10]*tx[ 6] + rx[14]*tx[ 7]; 
	modelview[ 7] = rx[3]*tx[ 4] + rx[7]*tx[ 5] + rx[11]*tx[ 6] + rx[15]*tx[ 7];

	modelview[ 8] = rx[0]*tx[ 8] + rx[4]*tx[ 9] + rx[ 8]*tx[10] + rx[12]*tx[11];
	modelview[ 9] = rx[1]*tx[ 8] + rx[5]*tx[ 9] + rx[ 9]*tx[10] + rx[13]*tx[11];
	modelview[10] = rx[2]*tx[ 8] + rx[6]*tx[ 9] + rx[10]*tx[10] + rx[14]*tx[11]; 
	modelview[11] = rx[3]*tx[ 8] + rx[7]*tx[ 9] + rx[11]*tx[10] + rx[15]*tx[11];
    
	modelview[12] = rx[0]*tx[12] + rx[4]*tx[13] + rx[ 8]*tx[14] + rx[12]*tx[15];
	modelview[13] = rx[1]*tx[12] + rx[5]*tx[13] + rx[ 9]*tx[14] + rx[13]*tx[15];
	modelview[14] = rx[2]*tx[12] + rx[6]*tx[13] + rx[10]*tx[14] + rx[14]*tx[15]; 
	modelview[15] = rx[3]*tx[12] + rx[7]*tx[13] + rx[11]*tx[14] + rx[15]*tx[15];
	// Modelview is ready to be loaded into opengl
}


double* PastyCam::getMatrix()
{
	return modelview;
}
